from __future__ import print_function
from pylab import *

def pplot(x,*a,**kw):
    if len(x.shape)>1:
        plot(x[:,0], x[:,1], *a, **kw)
    else:
        plot(x[0], x[1], *a, **kw)

def plot_eli(ax,pts, *a,**kw):
    mu = pts.mean(0)
    C = cov((pts[:,:2]-mu).T)
    u,s,v = svd(C)

    ss = 3*sqrt(s)

    print(dot(diag(ss),v))
    print()
    print(ss)
    print(v)


    theta = mgrid[:2*pi+0.01:0.01]
    x = (c_[sin(theta), cos(theta)])

    X = mu + dot(dot(x,diag(ss)),v)
    ax.plot(X[:,0], X[:,1], *a,**kw)
    #ax.plot(mu[0],mu[1],'kx')



if __name__ == '__main__':
    ion()
    #N8 predios
    v,h,s = [loadtxt('a'+x+'.txt') for x in ['v','h','s']]

    # N8 ladrilhos
    # v = loadtxt('bv.txt')
    # h = loadtxt('bh.txt')
    # s = loadtxt('bs.txt')
    # q = loadtxt('lad_24.txt')
    
    #v,h,s = [loadtxt('apa09_'+x+'.txt') for x in ['v','h','s']]

    figure(1, figsize=(6,4))
    suptitle('Principal point estimates', fontweight='bold', size=16)

    plot_eli(gca(), v[:,:2], 'r--', lw=1, alpha=0.5)
    pplot(v, 'ro')
    text(475,325,'Landscape', ha='center')

    plot_eli(gca(), h[:,:2], 'g--', alpha=0.5)
    pplot(h, 'gs')
    text(472,317,'Portrait', ha='left')

    plot_eli(gca(), s[:,:2], 'y--', alpha=0.5)
    pplot(s, 'y^')
    text(470,327,'Both')

    # plot_eli(gca(), q[:,:2], 'b--', alpha=0.5)
    # pplot(q, 'bv')
    # text(494,372,'Both (tiles)')

    plot(970/2.,652/2.,'ks')
    text(970/2.,652/2.-.5,'Image center',ha='center')

    ## Plot mean removing each point, makes outlier evident
    # g=[]
    # for k in range(12):
    #     g.append( mean(s[[i for i in range(12) if i != k]],0))
    # g = array(g)
    # pplot(g[:,:2],'rx')

    # L = loadtxt('lad_12.txt')
    # pplot(L[:12,:2], 'r.')
    # pplot(L[12:24,:2], 'g.')

    axis('equal')
    # ylim(750,0)
    # xlim(0,1000)
    xlim(466,489)
    ylim(328, 314)

    # ylim(340,310)
    # xlim(472,502)
    grid()

    #savefig('apa09_pp.pdf', dpi=200)

    Np = v.shape[0]

    xx = mgrid[:Np] #/11.
    figure(2, figsize=(6,4))
    suptitle('Estimates of other parameters', fontweight='bold', size=16)
    #subplot(2,1,1)
    axes([0.1, 0.53, 0.8,0.32])
    title('Focal distance')
    plot(sort(v[:,2]),xx,'r-o')
    plot(sort(h[:,2]),xx,'g-s')
    plot(sort(s[:Np,2]),xx,'y-^')
    xlim(738,750)
    ylim(-1,12)
    grid()

    # subplot(2,1,2)
    axes([0.1, 0.1, 0.8,0.32])
    title('Radial distortion')
    plot(sort(v[:,3]),xx,'r-o')
    plot(sort(h[:,3]),xx,'g-s')
    plot(sort(s[:Np,3]),xx,'y-^')
    xlim(1100,1600)
    ylim(-1,12)
    grid()

    #savefig('apa09_fd.pdf', dpi=200)
